#include "bga/graphics/CameraFreeFly.h"
#include <GL/glew.h>
#include <cmath>
#include <iostream>
#include "bga/math/Matrix.h"

#include <sstream>
#define INTENSITE 0.3

using namespace std;

namespace bga
{

CameraFreeFly::CameraFreeFly(const Vector3d& position, const Vector3d& center, const Vector3d& up) : Camera(position, CameraType::FREEFLY)
{
    initializeQuaternionDirection(position, center, up);
    buildTransformationMatrix();

    #ifdef DEBUG
    cout<<this->position.getX()<<";"<<this->position.getY()<<";"<<this->position.getZ()<<endl;
    #endif
}

CameraFreeFly::~CameraFreeFly()
{
}

void CameraFreeFly::onMouseMoved(int dmx, int dmz)
{
    this->rotateX(-(dmz*M_PI)/180);
    this->rotateY(-(dmx*M_PI)/180);
}

void CameraFreeFly::onKeyDown(CameraDirection::Direction key)
{
    switch(key)
    {
        case CameraDirection::UP:
            this->translateZ(-INTENSITE);
            break;

        case CameraDirection::DOWN:
            this->translateZ(+INTENSITE);
            break;

        case CameraDirection::LEFT:
            this->translateX(-INTENSITE);
            break;

        case CameraDirection::RIGHT:
            this->translateX(+INTENSITE);
            break;

        case CameraDirection::FORWARD:
            this->translateY(-INTENSITE);
            break;

        case CameraDirection::BACKWARD:
            this->translateY(+INTENSITE);
            break;

        default:
            break;
    }
    #ifdef DEBUG
    //cout<<this->position.getX()<<";"<<this->position.getY()<<";"<<this->position.getZ()<<endl;
    #endif
}

void CameraFreeFly::translateX( double intensite )
{
    this->direction.normalize();
    this->position = this->position + (this->direction.getConjugation() * Vector3d(1.0, 0.0, 0.0) * this->direction).toVector3D() * intensite;
    this->buildTransformationMatrix();
}

void CameraFreeFly::translateY( double intensite )
{
    this->position = this->position + (this->direction.getConjugation() * Vector3d(0.0, 1.0, 0.0) * this->direction).toVector3D() * intensite;
    this->buildTransformationMatrix();
}

void CameraFreeFly::translateZ( double intensite )
{
    this->position = this->position + (this->direction.getConjugation() * Vector3d(0.0, 0.0, 1.0) * this->direction).toVector3D() * intensite;
    this->buildTransformationMatrix();
}

void CameraFreeFly::rotate( double angle, double axisX, double axisY, double axisZ )
{
    /// Not exact
    rotateX(angle);
    rotateY(angle);
    rotateZ(angle);
}

void CameraFreeFly::rotateX( double angle )
{
    Quaternion qr(angle, 1.f, 0.f, 0.f, true);
    this->direction = qr*this->direction;
    this->buildTransformationMatrix();
}

void CameraFreeFly::rotateY( double angle )
{
    Quaternion qr(angle, 0.f, 1.f, 0.f, true);
    //this->direction = qr * this->direction;
    this->direction = this->direction*qr;
    this->buildTransformationMatrix();
}

void CameraFreeFly::rotateZ( double angle )
{
    Quaternion qr(angle, 0.f, 0.f, 1.f, true);
    //this->direction = this->direction*qr;
    this->direction = qr * this->direction;
    this->buildTransformationMatrix();
}

void CameraFreeFly::buildTransformationMatrix()
{

    this->direction.normalize();
    double s = this->direction.getS();
    double x = this->direction.getX();
    double y = this->direction.getY();
    double z = this->direction.getZ();

    /*this->mat_modelView[0] = 1 - 2 * y * y - 2 * z * z;
    this->mat_modelView[1] = 2 * x * y + 2 * s * z;
    this->mat_modelView[2] = 2 * x * z - 2 * s * y;
    this->mat_modelView[3] = 0;
    this->mat_modelView[4] = 2 * x * y - 2 * s * z;
    this->mat_modelView[5] = 1 - 2 * x * x - 2 * z * z;
    this->mat_modelView[6] = 2 * y * z + 2 * s * x;
    this->mat_modelView[7] = 0;
    this->mat_modelView[8] = 2 * x * z + 2 * s * y;
    this->mat_modelView[9] = 2 * y * z - 2 * s * x;
    this->mat_modelView[10] = 1- 2 * x * x - 2 * y * y;
    this->mat_modelView[11] = 0;
    this->mat_modelView[12] = -this->position.getX();
    this->mat_modelView[13] = -this->position.getY();
    this->mat_modelView[14] = -this->position.getZ();
    this->mat_modelView[15] = 1.f;*/

    this->modelViewMatrix(0, 0) = 1 - 2 * y * y - 2 * z * z;
    this->modelViewMatrix(0, 1) = 2 * x * y + 2 * s * z;
    this->modelViewMatrix(0, 2) = 2 * x * z - 2 * s * y;
    this->modelViewMatrix(0, 3) = 0;
    this->modelViewMatrix(1, 0) = 2 * x * y - 2 * s * z;
    this->modelViewMatrix(1, 1) = 1 - 2 * x * x - 2 * z * z;
    this->modelViewMatrix(1, 2) = 2 * y * z + 2 * s * x;
    this->modelViewMatrix(1, 3) = 0;
    this->modelViewMatrix(2, 0) = 2 * x * z + 2 * s * y;
    this->modelViewMatrix(2, 1) = 2 * y * z - 2 * s * x;
    this->modelViewMatrix(2, 2) = 1- 2 * x * x - 2 * y * y;
    this->modelViewMatrix(2, 3) = 0;
    this->modelViewMatrix(3, 0) = -this->position.getX();
    //this->modelViewMatrix(13) = -this->position.getY();
    this->modelViewMatrix(3, 1) = -this->position.getY();
    this->modelViewMatrix(3, 2) = -this->position.getZ();
    this->modelViewMatrix(3, 3) = 1.f;

    /*this->mat_pos[0] = 1.f;
    this->mat_pos[1] = 0.f;
    this->mat_pos[2] = 0.f;
    this->mat_pos[3] = 0.f;
    this->mat_pos[4] = 0.f;
    this->mat_pos[5] = 1.f;
    this->mat_pos[6] = 0.f;
    this->mat_pos[7] = 0.f;
    this->mat_pos[8] = 0.f;
    this->mat_pos[9] = 0.f;
    this->mat_pos[10] = 1.f;
    this->mat_pos[11] = 0.f;
    this->mat_pos[12] = -this->position.getX();
    this->mat_pos[13] = -this->position.getY();
    this->mat_pos[14] = -this->position.getZ();
    this->mat_pos[15] = 1.f;*/

    this->positionMatrix(0, 0) = 1.f;
    this->positionMatrix(0, 1) = 0.f;
    this->positionMatrix(0, 2) = 0.f;
    this->positionMatrix(0, 3) = 0.f;
    this->positionMatrix(1, 0) = 0.f;
    this->positionMatrix(1, 1) = 1.f;
    this->positionMatrix(1, 2) = 0.f;
    this->positionMatrix(1, 3) = 0.f;
    this->positionMatrix(2, 0) = 0.f;
    this->positionMatrix(2, 1) = 0.f;
    this->positionMatrix(2, 2) = 1.f;
    this->positionMatrix(2, 3) = 0.f;
    this->positionMatrix(3, 0) = -position.getX();
    this->positionMatrix(3, 1) = -position.getY();
    this->positionMatrix(3, 2) = -position.getZ();
    this->positionMatrix(3, 3) = 1.f;

    ///changement de repère 1 (global -> camera)
    this->modelViewMatrix = this->modelViewMatrix * this->positionMatrix;

    ///changement de repère 2 (retour)
    this->positionMatrix(3, 0) = position.getX();
    this->positionMatrix(3, 1) = position.getY();
    this->positionMatrix(3, 2) = position.getZ();

    this->modelViewMatrix = this->modelViewMatrix * this->positionMatrix;

    #ifdef DEBUG
    //cout<<this->toString()<<endl;
    #endif
}

Vector3d CameraFreeFly::getOrientation() const
{
    return (this->direction.getConjugation()* Vector3d(0, 0, -1) * this->direction).toVector3D();
}

Vector3d CameraFreeFly::getPosition() const
{
    return this->position.toVector3D();
}


Vector3d CameraFreeFly::getMouseOrientation(double x, double y)
{
    Vector3d v= (this->direction.getConjugation()*(Vector3d(0,0,-this->near)+Vector3d(x,y,0))*this->direction).toVector3D();
    v.normalize();
    return v;
}

}
